#ifndef GTRACK_H__
#define GTRACK_H__

// std
#include <stdint.h>

#if !defined(GTRACK_2D) && !defined(GTRACK_3D)
#define GTRACK_2D
#endif

/**
 * @brief GTRACK 算法的基本错误代码
 */
#define GTRACK_ERRNO_BASE (-8000)

 /**
  * @brief 错误代码：无错误
  */
#define GTRACK_EOK (0)

/**
* @brief 错误代码：无效参数
*/
#define GTRACK_EINVAL (GTRACK_ERRNO_BASE - 1)

/**
* @brief 错误代码: 由于前一个操作尚未完成，因此无法执行操作。
*/
#define GTRACK_EINUSE (GTRACK_ERRNO_BASE - 2)

/**
* @brief 错误代码: 未执行操作。
*/
#define GTRACK_ENOTIMPL (GTRACK_ERRNO_BASE - 3)

/**
* @brief 错误代码: 内存不足
*/
#define GTRACK_ENOMEM (GTRACK_ERRNO_BASE - 4)

/**
* @name 最大支持配置
*/
#define GTRACK_NUM_POINTS_MAX (1024U) /**< @brief 定义算法在配置时可接受的最大测量点数量。 */
#define GTRACK_NUM_TRACKS_MAX (250U) /**< @brief 定义算法在配置时可接受的最大追踪目标数量。 */

/**
* @name 目标 ID 定义
* 目标 ID 为 uint8_t，有效 ID 为 0 至 249。值 250 至 252 保留供将来使用，其他值定义如下：
*/
#define GTRACK_ID_POINT_TOO_WEAK        (253U) /**< @brief 点没有关联，太弱 */
#define GTRACK_ID_POINT_BEHIND_THE_WALL (254U) /**< @brief 点没有关联，位于框外 */
#define GTRACK_ID_POINT_NOT_ASSOCIATED  (255U) /**< @brief 点没有关联，噪音 */

/**
* @name 基准结果
*  在运行时执行过程中，track_step函数可选择返回以下子函数的周期计数
*  每个计数都是 32 位无符号整数值，代表自由运行时钟的时间戳
*/
#define GTRACK_BENCHMARK_SETUP     (0U) /**< @brief 函数运行开始时的周期计数 */
#define GTRACK_BENCHMARK_PREDICT   (1U) /**< @brief predict函数之后的周期计数 */
#define GTRACK_BENCHMARK_ASSOCIATE (2U) /**< @brief associate函数之后的周期计数 */
#define GTRACK_BENCHMARK_ALLOCATE  (3U) /**< @brief allocate函数之后的周期计数 */
#define GTRACK_BENCHMARK_UPDATE    (4U) /**< @brief update函数之后的周期计数 */
#define GTRACK_BENCHMARK_REPORT    (5U) /**< @brief report函数之后的周期计数 */

#define GTRACK_BENCHMARK_SIZE (GTRACK_BENCHMARK_REPORT) /**< @brief Size of benchmarking array */

// local
#ifdef GTRACK_3D
#include "gtrack_3d.h"
#else
#include "gtrack_2d.h"
#endif

#ifdef GTRACK_3D
/* Acceptable Resource Limits for 3D Tracker @ C67 */
#define LIMIT_PROG_MEMORY_SIZE        31000 /* shall take < 31k of proggram memory */
#define LIMIT_DATA_MEMORY_SIZE_MODULE 3200 /* 3.2k data memory for module */
#define LIMIT_DATA_MEMORY_SIZE_UNIT   1300 /* 1.3k of data memory per unit, so 20 tracks shall take < 3.2k+20*1.3 = 29.2k data memory  */
#define LIMIT_USEC_PER_TRACK_AVE      120 /* shall take < 120us per track at average */
#define LIMIT_USEC_PER_TRACK_MAX      200 /* shall take < 200us per track at maximum */
#else
/* Acceptable Resource Limits for 2D Tracker @ C67 */
#define LIMIT_PROG_MEMORY_SIZE        29000 /* shall take  < 29k of program memory */
#define LIMIT_DATA_MEMORY_SIZE_MODULE 2800 /* 3k data memory for module */
#define LIMIT_DATA_MEMORY_SIZE_UNIT   800 /* 0.8k of data memory per unit, so 20 tracks shall take < 2.8k+20*0.8 = 18.8k data memory  */
#define LIMIT_USEC_PER_TRACK_AVE      80 /* shall take < 80us per tarck at average */
#define LIMIT_USEC_PER_TRACK_MAX      120 /* shall take < 120us per track at maximum */
#endif

/**
 * @brief
 *  GTRACK 状态向量
 */
typedef enum
{
    /**  @brief   2D 恒速运动模型. 状态向量有4个变量 S={X, Y, Vx, Vy} */
    GTRACK_STATE_VECTORS_2DV = 0,
    /**  @brief   2D 恒加速运动模型. 状态向量有6个变量 S={X, Y, Vx, Vy, Ax, Ay} */
    GTRACK_STATE_VECTORS_2DA,
    /**  @brief   3D 恒速运动模型. 状态向量有6个变量 S={X, Y, Z, Vx, Vy, Vz} */
    GTRACK_STATE_VECTORS_3DV,
    /**  @brief   3D 恒加速运动模型. 状态向量有9个变量 S={X, Y, Z, Vx, Vy, Vz, Ax, Ay, Az} */
    GTRACK_STATE_VECTORS_3DA
} GTRACK_STATE_VECTOR_TYPE;

/**
 * @brief
 *  GTRACK 门限约束
 */
typedef struct
{
    /**  @brief   长度约束, m */
    float depth;
    /**  @brief   宽度约束, m */
    float width;
    /**  @brief   高度约束, m */
    float height;
    /**  @brief   径向速度约束, m/s */
    float vel;
} GTRACK_gateLimits;

/**
 * @brief
 *  GTRACK 门限函数参数
 */
typedef struct
{
    /**  @brief   门限函数的增益 */
    float gain;
    /**  @brief   门限函数的约束 */
    union
    {
        GTRACK_gateLimits limits;
        float             limitsArray[4];
    };
} GTRACK_gatingParams;

/**
 * @brief
 *  GTRACK 分配函数参数
 */
typedef struct
{
    /**  @brief 总SNR的阈值 */
    float snrThre;
    /**  @brief  速度阈值, m/s */
    float velocityThre;
    /**  @brief  点数阈值 */
    uint16_t pointsThre;
    /**  @brief  点与点之间的最大距离 */
    float maxDistanceThre;
    /**  @brief  点之间的最大速度差 */
    float maxVelThre;
} GTRACK_allocationParams;

/**
 * @brief
 * GTRACK 展开(Unrolling) 参数
 */
typedef struct
{
    /**  @brief  Range rate filtering alpha 距离变化率滤波alpha */
    float alpha;
    /**  @brief  Range rate filtering confidence 距离变化率滤波置信度 */
    float confidence;
} GTRACK_unrollingParams;

/**
 * @brief
 * GTRACK 轨迹合并参数
 */
typedef struct
{
    /** 满足合并条件的次数达到该阈值后进行轨迹合并 */
    uint16_t mergeCountThre;
    /** 合并轨迹的距离差异阈值 */
    float mergeDistThre;
    /** 合并轨迹的速度差异阈值 */
    float mergeSpeedThre;
} GTRACK_mergeParams;

typedef struct
{
    /**  @brief   传感器输出的最大径向速度， +/- m/s */
    float maxRadialVelocity;
    /**  @brief   径向速度分辨率, m/s */
    float radialVelocityResolution;
    /**  @brief   距离分辨率, m */
    float rangeResolution;
    /**  @brief   角度分辨率，rad */
    float radResolution;
    /**  @brief   帧率, s */
    float deltaT;

} GTRACK_RadarParams;

/**
 * @name 边界框
 */
#define GTRACK_MAX_BOUNDARY_BOXES (3U) /**< @brief 边界框的最大数量，边界框外的点将被忽略。 */
#define GTRACK_MAX_STATIC_BOXES   (3U) /**< @brief 静态框的最大数量，静态框内的目标可持续更长时间。 */

/**
 * @brief
 *  GTRACK Box结构体
 */
typedef struct
{
    union
    {
        struct
        {
            /**  @brief   左边界, m */
            float x1;
            /**  @brief   右边界, m */
            float x2;
            /**  @brief   近端边界, m */
            float y1;
            /**  @brief   远端边界, m */
            float y2;
            /**  @brief   底部边界, m */
            float z1;
            /**  @brief   顶部边界, m */
            float z2;
        };
        float box[6];
    };

} GTRACK_boundaryBox;

/**
 * @brief
 *  GTRACK Scene参数
 *
 * @details
 *  这组参数描述了Scene。
 *  它允许用户配置跟踪器的预期边界和静态行为区域。
 *  用户最多可以定义 GTRACK_MAX_BOUNDARY_BOXES 边界框，以及 GTRACK_MAX_STATIC_BOXES 静态框。
 *  方框坐标以米为单位，传感器假定位于笛卡尔（X，Y）空间的（0，0）处。
 */
typedef struct
{
    /**  @brief 场景边界框的数量。如果已定义（numBoundaryBoxes > 0），则只有边界框内的点才能与轨迹相关联。 */
    uint8_t numBoundaryBoxes;
    /**  @brief 场景边界框 */
    GTRACK_boundaryBox boundaryBox[GTRACK_MAX_BOUNDARY_BOXES];
    /**  @brief 场景静态框的数量。如果已定义（numStaticBoxes > 0），则只有静态框内的目标可以作为静态目标持续存在。 */
    uint8_t numStaticBoxes;
    /**  @brief 场景静态框 */
    GTRACK_boundaryBox staticBox[GTRACK_MAX_STATIC_BOXES];
} GTRACK_sceneryParams;

/**
 * @brief
 *  GTRACK 高级参数
 */
typedef struct
{
    /**  @brief  Pointer to gating parameters 指向门控参数的指针 */
    GTRACK_gatingParams* gatingParams;
    /**  @brief  Pointer to allocation parameters 指向分配参数的指针 */
    GTRACK_allocationParams* allocationParams;
    /**  @brief  Pointer to unrolling parameters 指向展开参数的指针 */
    GTRACK_unrollingParams* unrollingParams;
    /** @brief 轨迹合并参数的指针 */
    GTRACK_mergeParams* mergeParams;
    /**  @brief 指向场景参数的指针 */
    GTRACK_sceneryParams* sceneryParams;
} GTRACK_advancedParameters;

/* gtrack 算法参数 */
typedef struct
{
    /**  @brief   状态向量类型 */
    GTRACK_STATE_VECTOR_TYPE stateVectorType;
    /**  @brief   每帧测量点的最大个数. Up to \ref GTRACK_NUM_POINTS_MAX supported */
    uint16_t maxNumPoints;
    /**  @brief   最大的跟踪目标个数. Up to \ref GTRACK_NUM_TRACKS_MAX supported */
    uint16_t maxNumTracks;
    /**  @brief   初始目标径向速度， m/s */
    float initialRadialVelocity;
    /**  @brief   预期最大目标加速度 (X，Y，Z)方向, m/s2 */
    float maxAcceleration[3];
    /**  @brief   雷达参数 */
    GTRACK_RadarParams radarParams;
    /**  @brief  高级参数, 默认为NULL */
    GTRACK_advancedParameters* advParams;

} GTRACK_moduleConfig;

#define GTRACK_STATE_VECTOR_SIZE       (sizeof(GTRACK_state_vector_pos_vel_acc) / sizeof(float))
#define GTRACK_MEASUREMENT_VECTOR_SIZE (sizeof(GTRACK_measurement_vector) / sizeof(float))

/**
 * @brief
 *  GTRACK 测量点
 */
typedef struct
{
    union
    {
        /**  @brief   测量向量 */
        GTRACK_measurement_vector vector;
        float                     array[GTRACK_MEASUREMENT_VECTOR_SIZE];
    };
    /**  @brief   信噪比 , linear */
    float snr;
    /**  @brief   角度信噪比，linear */
    float doa_snr;
    ///**  @brief  能量/RCS */
    //float power;
} GTRACK_measurementPoint;

typedef union
{
    /**  @brief   测量向量 */
    GTRACK_measurement_vector vector;
    float                     array[GTRACK_MEASUREMENT_VECTOR_SIZE];
} GTRACK_measurementUnion;

/**
* @brief
*  GTRACK单元状态
*
* @details
*  该结构描述了 GTRACK 单元的状态
*/
typedef enum
{
    /**  @brief Free (not allocated) */
    TRACK_STATE_FREE = 0,
    /**  @brief INIT */
    TRACK_STATE_INIT,
    /**  @brief DETECTION State */
    TRACK_STATE_DETECTION,
    /**  @brief ACTIVE State */
    TRACK_STATE_ACTIVE
} TrackState;

/**
 * @brief
 *  GTRACK 目标描述符
 */
typedef struct
{
    /**  @brief   所在车道编号 1~255 */
    uint8_t road_id;
    /**  @brief   跟踪器标识符 */
    uint8_t uid;
    /**  @brief   目标标识符 */
    uint16_t tid;
    /**  @brief   状态向量 */
    float S[GTRACK_STATE_VECTOR_SIZE];
    /**  @brief   估计目标维度: 长, 宽, [height], 速度 ？？*/
    float dim[GTRACK_MEASUREMENT_VECTOR_SIZE];
    /**  @brief  轨迹置信度 */
    float score;
    /**  @brief   轨迹状态 */
    TrackState state;
} GTRACK_targetDesc;

/**
*  @b Description
*  @n
*   算法级create函数
*       应用程序调用该函数创建一个带有所需配置参数的 GTRACK 算法实例。
*       函数返回一个句柄，用于执行单帧gtrack_step函数或gtrack_delete函数。
*  @param[in] config
*		这是一个指向配置结构的指针。该结构包含 GTRACK 算法暴露的所有参数。
*		配置无需持续存在。
*		高级配置结构可设置为 NULL 以使用默认结构。
*		高级配置中的任何字段也可以设置为 NULL，以使用该字段的默认值。
*  @param[out] errCode
*      出错时弹出的错误代码，参见 GTRACK_ERROR_CODE
*
*  @retval
*      GTRACK模块的句柄
*/
void* gtrack_create(GTRACK_moduleConfig* config, int32_t* errCode);

/**
 *  @b Description
 *  @n
 *   算法级step函数
 *       应用程序应调用该函数来处理一帧测量结果，并使用给定的算法实例。
 *
 *  @param[in]  handle
 *     GTRACK模块的句柄
 *  @param[in]  point
 *      输入测量值数组的指针。每个测量值都包含距离/角度/径向速度信息。
 *  @param[in]  var
 *      输入测量偏差数组的指针。如果变量未知，则应设置为 NULL
 *  @param[in]  mNum
 *      测量值的个数
 *  @param[out]  t
 *      指向 GTRACK_targetDesc 数组的指针。应用程序应为预期的目标数量提供足够的空间。
 *      该函数为每个被跟踪的目标填充描述信息。
 *  @param[out]  tNum
 *      指向 uint16_t 值的指针。
 *      函数返回已填充目标描述符的数量。
 *  @param[out]  mIndex
 *      指向 uint8_t 指数数组的指针。应用程序应提供足够的空间来索引所有测量点。
 *      该函数填充目标索引，显示为每个测量值分配的跟踪 ID。参见目标 ID 失败定义，示例 GTRACK_ID_POINT_NOT_ASSOCIATED
 *	    当不需要索引时，应设为 NULL。
 *
 *  @retval
 *      None
 */
void  gtrack_step(void* handle, GTRACK_measurementPoint* point, GTRACK_measurement_vector* var, uint16_t mNum, GTRACK_targetDesc* t, uint16_t* tNum, uint8_t* mInde);


/**
 *  @b Description
 *  @n
 *	 算法级delete函数
 *      应用程序调用该函数删除 GTRACK 算法实例。
 *
 *  @param[in]  handle
 *      GTRACK模块的句柄
 *
 *  @retval
 *      None
 */
void  gtrack_delete(void* handle);

 /**
  * @brief
  *  GTRACK 调用该函数分配内存。如果分配成功，则期望得到 void 指针，否则为 NULL。
  *
  *  @param[in]  numElements
  *      要分配的元素数量
  *  @param[in]  sizeInBytes
  *      要分配的每个元素的大小（以字节为单位）
  */
void* gtrack_alloc(uint32_t numElements, uint32_t sizeInBytes);

/**
 * @brief
 *  GTRACK 调用该函数释放内存
 *
 *  @param[in]  pFree
 *      指向要释放的内存的指针
 *  @param[in]  sizeInBytes
 *      要释放的内存大小（以字节为单位）
 */
void gtrack_free(void* pFree, uint32_t sizeInBytes);

/**
 * @brief
 *  GTRACK 调用该函数来记录日志
 *
 *  @param[in]  level
 *      日志的重要性
 *  @param[in]  format
 *      Format 是大小可变的格式化输出
 */
void gtrack_log(const char* format, ...);

#endif // !GTRACK_H__